﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_ObjectIdentification.cs
 * \brief	This file contains the ObjectIdentification function that 
 *  
 * This file contains the ObjectIdentification function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2010-01-11
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo Rodriguez                         2010-01-11 \n
 *      First implementation of the M44_CompareReconstr.
 *      
 * - Version 0.02:   Gonzalo Rodriguez                         2010-01-11 \n
 *      Change the iteration limits.
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
/*These are for plotting files*/
using System.Globalization;
using System.IO;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_isMovement
     * 
     *  \brief  This is the class of the M44_isMovement function.
     * 
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_isMovement
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static void isMovement()
         * 
         *  \brief 
         *  \param[in] aux_MapCell LocalMapCell                 actual Mapcell
         *  \param[in] aux_MapCell LocalMapCell_ant             last mapcell
         *  \param[in] bool [,] ant_MovementMap                 Last movement map
         *      
         *  \param[out]  isMovementStruct MovementData          MovementData structure with actual movement map and a variable indicating if there has been movement
         */
        /* ------------------------------------------------------------------------- */
        public static isMovementStruct isMovement(aux_MapCell LocalMapCell,
                                        isMovementStruct MovementData)
        {
            bool[,] MovementMap = new bool[LocalMapCell.us_Nrows,LocalMapCell.us_Ncols];
            bool isMovement_global = false;
            bool isMovement_cell = false;
            int aux_i,aux_j;
            int[] u1 = {0, 0, 1, 1, 1, 0, -1, -1, -1 };
            int[] u2 = {0, 1, 1, 0, -1, -1, -1, 0, 1 };

            MovementMap = M44_CompareReconst.CompareReconst(LocalMapCell, MovementData.MapCellMatrix_k);
            //Check both movement maps for continuous movement
            for (int i = 0; i < LocalMapCell.us_Nrows; i++)
            {
                for (int j = 0; j < LocalMapCell.us_Ncols; j++)
                {
                    MovementData.tb_MovementMap_k[i, j] = false;
                    if (MovementMap[i, j])
                    {
                        isMovement_cell = false;
                        for (int k = 0; k < 9; k++)
                        {
                            aux_i = i + u1[k];
                            aux_j = j + u2[k];
                            if (aux_i >= 0 && aux_i < LocalMapCell.us_Nrows &&
                                aux_j >= 0 && aux_j < LocalMapCell.us_Ncols)
                            {
                                if (MovementData.tb_MovementMap_k_1[aux_i, aux_j])
                                    isMovement_cell = true;
                            }
                        }
                        if (isMovement_cell)
                        {
                            MovementData.tb_MovementMap_k[i, j] = true;
                            isMovement_global = true;
                        }
                    }

                }
            }
            MovementData.b_isMovement = isMovement_global;
            MovementData.tb_MovementMap_k_1 = MovementMap;
            MovementData.MapCellMatrix_k = LocalMapCell;

            return MovementData;
        }


        public static void CreatePlotFile(string RobotId,string Type, bool[,] MovementMap,cl_Map2D Map,float time)
        {

            /*It changes the decimal sign to a point*/
            NumberFormatInfo nfi = new NumberFormatInfo();
            nfi.NumberDecimalSeparator = ".";
            nfi.NumberDecimalDigits = 8;
            float[] Pos = new float[2];
            int[] coord = new int[2];
            int aux = 0;
            /*It creates filter.m file and stores the desired values*/
            for (int i = 0; i < Map.us_Nrows; i++)
            {
                for (int j = 0; j < Map.us_Ncols; j++)
                {
                    if (MovementMap[i, j])// && antMovementMap[i,j])
                    {
                        coord[0] = i;
                        coord[1] = j;
                        Pos = M44_BasicClass.Cell2Coordinates_High(Map, coord);
                        using (StreamWriter w = File.AppendText("C:\\NMRS\\M44_SensorFusionAndMapping\\Plot-files\\" + RobotId + Type + ".m"))
                        {
                            w.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}",
                            time.ToString("N", nfi),
                            Pos[0].ToString("N", nfi),
                            Pos[1].ToString("N", nfi),
                            Map.Center.d_x.ToString("N",nfi),
                            Map.Center.d_y.ToString("N",nfi),
                            aux.ToString("N",nfi)
                            );

                            /*Update the underlying file.*/
                            w.Flush();

                            /*Close the writer and underlying file.*/
                            w.Close();
                        }
                    }
                }
            }
            aux = 1;
        }
    }
}